#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "include/libmod.h"
#include "include/libcore.h"
#include "include/modid.h"
#include "include/vmerr.h"
#include "include/nr_excp.h"
#include "mod_rv32g.h"

#define FETCH_PC (req.saved_pc)
#define CLR_X0 (req.env->cpu.regs[x0]=0)
#define PC (req.env->cpu.pc)
#define IR (this->ir) 
#define ILEN16(ir) ((ir&0x3)!=0x3)
#define ILEN32(ir) ((ir&0x3)==0x3 && (ir&0x7)!=0x7)
#define X (req.env->cpu.regs)
#define MADDR (this->maddr)
#define MBUF (this->mbuf)
#define NEXT_PC (PC+=4)
#define XL5(x) (x&0x1F)
#define MEM(env,addr,send,recv,rw) (this->vm_enable?memrw_virt(env, addr, send, &recv, rw):memrw_phy(env, addr, send, &recv, rw))
#define BIT8_MASK 0xFF
#define BIT16_MASK  0xFFFF

struct rv32g_ctx {
    word ir;
    word mbuf;
    memaddr_t maddr;
    word csrbuf;
    bool vm_enable;
    bool decode_enable;
    bool rv32i_enable;
    bool rv32m_enable;
    bool rv32zicsr_enable;
    word t0, t1;
    word pc_inc;
    struct {
        word mstatus;
        word mie;
        word mtvec;
        word mscratch;
        word mepc;
        word mtval;
        word mip;
        word matp;
        word mtime;
        word mtimecmp;
        word mcause;
        word test;
    } csr;
};

status rv32g_init (struct environ * env, struct module * mod); 
status rv32g_start (struct environ * env, struct module * mod); 
status rv32g_stop (struct environ * env, struct module * mod); 
status rv32g_pause (struct environ * env, struct module * mod); 
status rv32g_resume (struct environ * env, struct module * mod); 
status rv32g_ctrl (struct environ * env, struct module * mod, uint64 arg);
status rv32i_decoder (struct cpu_decoder_req req);
status rv32m_decoder (struct cpu_decoder_req req);
status rv32zicsr_decoder (struct cpu_decoder_req req);
status rv32g_exhdlr (struct cpu_exhdlr_req req);
status rv32zicsr_csrrw (struct cpu_csrrw_req req);
static void csr_refresh (struct rv32cpu* cpu, struct rv32g_ctx * this);
static void csr_apply (struct rv32cpu* cpu, struct rv32g_ctx * this);
status fetch_instruction (struct cpu_decoder_req req);

status
rv32g_assemble (struct module * mod) {
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * ctx = (struct rv32g_ctx*)malloc (sizeof (struct rv32g_ctx));
    bzero (ctx,sizeof (struct rv32g_ctx));
    if (ctx == NULL) {
        stat |= ERR_NOMEM;
        goto error;
    }
    mod->context = ctx;
    mod->init = rv32g_init;
    mod->start = rv32g_start;
    mod->stop = rv32g_stop;
    mod->pause = rv32g_pause;
    mod->resume = rv32g_resume;
    mod->modctl = rv32g_ctrl;
    mod->stat = MOD_STAT_UNINITIALIZED;
    mod->modid = RV32GISA_ID;
    strncpy (mod->name, "RV32IM+Zicsr", MAX_MOD_NAMESZ);
    stat |= module_add_item (mod, MI_CPU_DECODER, rv32i_decoder);
    stat |= module_add_item (mod, MI_CPU_DECODER, rv32m_decoder);
    stat |= module_add_item (mod, MI_CPU_EXHDLR, rv32g_exhdlr);
    stat |= module_add_item (mod, MI_CPU_DECODER, rv32zicsr_decoder);
    stat |= module_add_item (mod, MI_CPU_CSRRW, rv32zicsr_csrrw);
    if (stat == STAT_SUCCESS)
        mod->magic = BISCUIT_MAGIC;
    return stat;
    error:
    return stat;
}

status
rv32g_init (struct environ * env, struct module * mod) {
    (void) env;
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * this = (struct rv32g_ctx *) mod->context;
    bzero (this, sizeof (struct rv32g_ctx));
    mod->stat = MOD_STAT_STOPPED;
    return stat;
}

status
rv32g_start (struct environ * env, struct module * mod) {
    (void) env;
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * this = (struct rv32g_ctx *) mod->context;
    if (mod->stat != MOD_STAT_STOPPED) {
        stat |= ERR_STATUS;
        goto error;
    }
    mod->stat = MOD_STAT_RUNNING;
    this->decode_enable = true;
    this->rv32i_enable = true;
    this->rv32m_enable = true;
    this->rv32zicsr_enable = true;
    return stat;
    error:
    return stat;
}

status
rv32g_stop (struct environ * env, struct module * mod) {
    (void) env;
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * this = (struct rv32g_ctx *) mod->context;
    if (mod->stat != MOD_STAT_RUNNING) {
        stat |= ERR_STATUS;
        goto error;
    }
    mod->stat = MOD_STAT_STOPPED;
    bzero (this, sizeof (struct rv32g_ctx));
    // this->running = false;
    return stat;
    error:
    return stat;
}

status
rv32g_pause (struct environ * env, struct module * mod) {
    (void) env;
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * this = (struct rv32g_ctx *) mod->context;
    if (mod->stat != MOD_STAT_RUNNING) {
        stat |= ERR_STATUS;
        goto error;
    }
    mod->stat = MOD_STAT_PAUSED;
    // bzero (this, sizeof (struct rv32g_ctx));
    this->decode_enable = false;
    return stat;
    error:
    return stat;
}

status
rv32g_resume (struct environ * env, struct module * mod) {
    (void) env;
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * this = (struct rv32g_ctx *) mod->context;
    if (mod->stat != MOD_STAT_PAUSED) {
        stat |= ERR_STATUS;
        goto error;
    }
    mod->stat = MOD_STAT_RUNNING;
    this->decode_enable = true;
    return stat;
    error:
    return stat;
}

#define RV32G_CTL_I_ENABLE 1
#define RV32G_CTL_M_ENABLE 2
#define RV32G_CTL_I_DISABLE 3
#define RV32G_CTL_M_DISABLE 4

status
rv32g_ctrl (struct environ * env, struct module * mod, uint64 arg) {
    (void) env;
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * this = (struct rv32g_ctx *) mod->context;
    switch (arg)
    {
    case RV32G_CTL_I_ENABLE: {
        if (this->rv32i_enable) {
            stat |= ERR_CTXSTAT;
            goto error;
        }
        this->rv32i_enable = true;
        break;
    }
    case RV32G_CTL_M_ENABLE: {
        if (this->rv32m_enable) {
            stat |= ERR_CTXSTAT;
            goto error;
        }
        this->rv32m_enable = true;
        break;
    }
    case RV32G_CTL_I_DISABLE: {
        if (!this->rv32i_enable) {
            stat |= ERR_CTXSTAT;
            goto error;
        }
        this->rv32i_enable = false;
        break;
    }
    case RV32G_CTL_M_DISABLE: {
        if (!this->rv32m_enable) {
            stat |= ERR_CTXSTAT;
            goto error;
        }
        this->rv32m_enable = false;
        break;
    }
    default:
        stat |= ERR_INVARG;
        break;
    }
    return stat;
    error:
    return stat;
}
#define FORCE_RET goto executed;

status
fetch_instruction (struct cpu_decoder_req req) {
    status stat = STAT_SUCCESS;
    struct rv32g_ctx * this = (struct rv32g_ctx *) req.ctx;
    if (FETCH_PC & 0x3) {
        stat |= ERR_MALIGN;
        raise_excep_sync (req.env, EXCP_INST_MISALIGN);
        goto error;
    }
    if (this->vm_enable)
        stat |= memrw_virt (req.env, FETCH_PC, 0, &IR, RW_X32);
    else
        stat |= memrw_phy (req.env, FETCH_PC, 0, &IR, RW_X32);
    return stat;
    error:
    return stat;
}

status
rv32i_decoder (struct cpu_decoder_req req) {
    //DBG_LOG (rv32i_decoder, "rv32i decoder");
    struct rv32g_ctx * this = (struct rv32g_ctx *) req.ctx;
    status stat = STAT_SUCCESS;
    if (!(this->decode_enable && this->rv32i_enable)) goto error;
    stat = fetch_instruction(req);
    // printf ("0x%X -> %X\n", FETCH_PC, IR);
    if (stat != STAT_SUCCESS) {this->csr.mtval = FETCH_PC; FORCE_RET;}
    if (OPCODE != 0x33) goto L1;

    DEF_R_INST (ADD, 0x0, 0x0, 0x33,{
        X[RD] = X[RS1] + X[RS2];
        NEXT_PC;
    });

    DEF_R_INST (SUB, 0x20, 0x0, 0x33,{
        X[RD] = X[RS1] - X[RS2];
        NEXT_PC;
    });

    DEF_R_INST (AND, 0x0, 0x7, 0x33, {
        X[RD] = X[RS1] & X[RS2];
        NEXT_PC;
    });

    DEF_R_INST (OR, 0x0, 0x6, 0x33, {
        X[RD] = X[RS1] | X[RS2];
        NEXT_PC;
    });

    DEF_R_INST (XOR, 0x0, 0x4, 0x33, {
        X[RD]= X[RS1] ^ X[RS2];
        NEXT_PC;
    });

    DEF_R_INST (SLL, 0x0, 0x1, 0x33, {
        X[RD] = U(X[RS1]) << XL5(U(X[RS2]));
        NEXT_PC;
    });

    DEF_R_INST (SRL, 0x0, 0x5, 0x33, {
        X[RD] = U(X[RS1]) >> XL5(U(X[RS2]));
        NEXT_PC;
    });

    DEF_R_INST (SRA, 0x20, 0x5, 0x33, {
        X[RD] = S(X[RS1]) >> XL5(U(X[RS2]));
        NEXT_PC;
    });

    DEF_R_INST (SLT, 0x0, 0x2, 0x33, {
        X[RD] = S(X[RS1]) < S(X[RS2]);
        NEXT_PC;
    });

    DEF_R_INST (SLTU, 0x0, 0x3, 0x33, {
        X[RD] = U(X[RS1]) < U(X[RS2]);
        NEXT_PC;
    });

    L1:
    if (OPCODE != 0x13) goto L2;
    DEF_I_INST (ADDI, 0x0, 0x13, {
        X[RD] = X[RS1] + SEXT(IMM_I);
        NEXT_PC;
    });

    DEF_I_INST (ANDI, 0x7, 0x13, {
        X[RD] = X[RS1] & SEXT(IMM_I);
        NEXT_PC;
    });

    DEF_I_INST (ORI, 0x6, 0x13, {
        X[RD] = X[RS1] | SEXT(IMM_I);
        NEXT_PC;
    });

    DEF_I_INST (XORI, 0x4, 0x13, {
        X[RD] = X[RS1] ^ SEXT(IMM_I);
        NEXT_PC;
    });

    DEF_I_SP_INST (SLLI, 0x0, 0x1, 0x13, {
        X[RD] = X[RS1] << SHAMT;
        NEXT_PC;
    });

    DEF_I_SP_INST (SRLI, 0x0, 0x5, 0x13, {
        X[RD] = U(X[RS1]) >> SHAMT;
        NEXT_PC;
    });

    DEF_I_SP_INST (SRAI, 0x20, 0x5, 0x13, {
        X[RD] = S(X[RS1]) >> SHAMT;
        NEXT_PC;
    });
    DEF_I_INST (SLTI, 0x2, 0x13, {
        X[RD] = S(X[RS1]) < SEXT(IMM_I);
        NEXT_PC;
    });
    DEF_I_INST (SLTIU, 0x3, 0x13, {
        X[RD] = U(X[RS1]) < ZEXT(IMM_I);
        NEXT_PC;
    });
    L2:
    if (OPCODE != 0x3) goto L3;
    DEF_I_INST (LB, 0x0, 0x3, {
        MADDR = X[RS1] + SEXT(IMM_I);
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_R8);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;} // 如果stat!=0,访存错误,异常已在访存模块发起
        X[RD] = SEXT8(MBUF);
        NEXT_PC;
    });

    DEF_I_INST (LH, 0x1, 0x3, {
        MADDR = X[RS1] + SEXT(IMM_I);
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_R16);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;}
        X[RD] = SEXT16(MBUF);
        NEXT_PC;
    });

    DEF_I_INST (LW, 0x2, 0x3, {
        MADDR = X[RS1] + SEXT(IMM_I);
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_R32);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;}
        X[RD] = MBUF;
        NEXT_PC;
    });

    DEF_I_INST (LBU, 0x4, 0x3, {
        MADDR = X[RS1] + SEXT(IMM_I);
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_R8);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;}
        X[RD] = ZEXT8(MBUF);
        NEXT_PC;
    });

    DEF_I_INST (LHU, 0x5, 0x3, {
        MADDR = X[RS1] + SEXT(IMM_I);
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_R16);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;}
        X[RD] = ZEXT16(MBUF);
        NEXT_PC;
    });
    L3:
    DEF_I_JMP_INST (JALR, 0x0, 0x67, {
        word t;
        t = PC + 4;
        PC = (X[RS1] + SEXT(IMM_I))&~1;
        // printf ("vm logger: jumping to 0x%X tracing stack: 0x%X\n", PC ,X[x2]);
        X[RD] = t;
    });
    if (OPCODE != 0x23) goto L4;
    DEF_S_INST (SB, 0x0, 0x23, {
        MADDR = X[RS1] + SEXT(IMM_S);
        MBUF = X[RS2] & BIT8_MASK;
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_W8);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;}
        NEXT_PC;
    });
    DEF_S_INST (SH, 0x1, 0x23, {
        MADDR = X[RS1] + SEXT(IMM_S);
        MBUF = X[RS2] & BIT16_MASK;
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_W16);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;}
        NEXT_PC;
    });
    DEF_S_INST (SW, 0x2, 0x23, {
        MADDR = X[RS1] + SEXT(IMM_S);
        MBUF = X[RS2];
        stat |= MEM (req.env, MADDR, MBUF, MBUF, RW_W32);
        if (stat) {this->csr.mtval = MADDR; FORCE_RET;}
        NEXT_PC;
    });
    L4:
    if (OPCODE != 0x63) goto L5;
    DEF_B_INST (BEQ, 0x0, 0x63, {
        if (X[RS1] == X[RS2])
            PC += SEXT(IMM_B);
        else
            NEXT_PC;
    });
    DEF_B_INST (BNE, 0x1, 0x63, {
        if (X[RS1] != X[RS2])
            PC += SEXT(IMM_B);
        else
            NEXT_PC;
    });
    DEF_B_INST (BLT, 0x4, 0x63, {
        if (S(X[RS1]) < S(X[RS2]))
            PC += SEXT(IMM_B);
        else
            NEXT_PC;
    });
    DEF_B_INST (BGE, 0x5, 0x63, {
        if (S(X[RS1]) >= S(X[RS2]))
            PC += SEXT(IMM_B);
        else
            NEXT_PC;
    });
    DEF_B_INST (BLTU, 0x6, 0x63, {
        if (U(X[RS1]) < U(X[RS2]))
            PC += SEXT(IMM_B);
        else
            NEXT_PC;
    });
    DEF_B_INST (BGEU, 0x7, 0x63, {
        if (U(X[RS1]) >= U(X[RS2]))
            PC += SEXT(IMM_B);
        else
            NEXT_PC;
    });
    L5:
    DEF_U_INST (LUI, 0x37, {
        X[RD] = (SEXT(IMM_U) << 12);
        NEXT_PC;
    });

    DEF_U_INST (AUIPC, 0x17, {
        X[RD] = PC + (SEXT(IMM_U) << 12);
        NEXT_PC;
    });

    DEF_J_INST (JAL, 0x6F, {
        X[RD] = PC + 4;
        PC += SEXT(IMM_J);
    });
    /* psudo instruction, RV32FD */
    DEF_I_INST (FLW, 0x2, 0x7, {});
    DEF_S_INST (FSW, 0x2, 0x27, {});
    DEF_I_INST (FLD, 0x3, 0x7, {});
    DEF_S_INST (FSD, 0x3, 0x27, {});
    goto error;
    executed:
    req.rstat->req_ack = true;
    return stat;
    error:
    return stat;
}

status
rv32m_decoder (struct cpu_decoder_req req) {
    struct rv32g_ctx * this = (struct rv32g_ctx *) req.ctx;
    status stat = STAT_SUCCESS;
    int64 tmpi;
    uint64 tmpu;
    if (!(this->decode_enable && this->rv32m_enable)) goto error;
    stat = fetch_instruction(req);
    if (stat) {this->csr.mtval = FETCH_PC; FORCE_RET;}
    DEF_R_INST (MUL, 0x1, 0x0, 0x33, {
        X[RD] = S(X[RS1]) * S(X[RS2]);
        NEXT_PC;
    });
    DEF_R_INST (MULH, 0x1, 0x1, 0x33, {
        tmpi = S(X[RS1]) * S(X[RS2]);
        X[RD] = S(tmpi >> 32);
        NEXT_PC;
    });
    DEF_R_INST (MULHSU, 0x1, 0x2, 0x33, {
        tmpi = S(X[RS1]) * U(X[RS2]);
        X[RD] = S(tmpi >> 32);
        NEXT_PC;
    });
    DEF_R_INST (MULHU, 0x1, 0x3, 0x33, {
        tmpu = U(X[RS1]) * U(X[RS2]);
        X[RD] = U(tmpu >> 32);
        NEXT_PC;
    });
    DEF_R_INST (DIV, 0x1, 0x4, 0x33, {
        if (!X[RS2]) {
            raise_excep_sync (req.env, EXCP_DIVZERO);
            stat |= ERR_MODFN;
            FORCE_RET;
        }
        X[RD] = S(X[RS1]) / S(X[RS2]);
        NEXT_PC;
    });
    DEF_R_INST (DIVU, 0x1, 0x5, 0x33, {
        if (!X[RS2]) {
            raise_excep_sync (req.env, EXCP_DIVZERO);
            stat |= ERR_MODFN;
            FORCE_RET;
        }
        X[RD] = U(X[RS1]) / U(X[RS2]);
        NEXT_PC;
    });
    DEF_R_INST (REM, 0x1, 0x6, 0x33, {
        if (!X[RS2]) {
            raise_excep_sync (req.env, EXCP_DIVZERO);
            stat |= ERR_MODFN;
            FORCE_RET;
        }
        X[RD] = S(X[RS1]) % S(X[RS2]);
        NEXT_PC;
    });
    DEF_R_INST (REMU, 0x1, 0x7, 0x33, {
        if (!X[RS2]) {
            raise_excep_sync (req.env, EXCP_DIVZERO);
            stat |= ERR_MODFN;
            FORCE_RET;
        }
        X[RD] = U(X[RS1]) % U(X[RS2]);
        NEXT_PC;
    });
    //stat |= ERR_DECODER_UNDEF;
    goto error;
    executed:
    req.rstat->req_ack = true;
    return stat;
    error:
    return stat;
}

status
rv32zicsr_decoder (struct cpu_decoder_req req) {
    struct rv32g_ctx * this = (struct rv32g_ctx *) req.ctx;
    status stat = STAT_SUCCESS;
    struct req_stat rstat;
    #define t0 (this->t0)
    #define t1 (this->t1)
    //word t0, t1;
    bzero (&rstat, sizeof (struct req_stat));
    if (!(this->decode_enable && this->rv32zicsr_enable)) goto error;
    if (req.env->cpu.mode == CPU_MODE_U) goto usermode;
    stat |= fetch_instruction(req);

    #define ILLGAL_INST_TESTER { \
    if (req.env->cpu.mode == CPU_MODE_U) { \
        raise_excep_sync (req.env, EXCP_ILLGAL_INST); \
        this->csr.mtval = FETCH_PC; \
        stat |= ERR_MODFN; \
        goto error;\
    }}
    //printf ("%X -> %X\n", FETCH_PC, IR);
    if (stat) {this->csr.mtval = FETCH_PC; FORCE_RET;}
    //printf ("\n");
    /* t = CSRs[csr]; CSRs[csr] = x[rs1]; x[rd] = t */
    DEF_I_INST (CSRRW, 0x1, 0x73, {
        ILLGAL_INST_TESTER;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, t0, &t0, RW_R32);
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, X[RS1], &X[RS1], RW_W32);
        if (stat) FORCE_RET;
        X[RD] = t0;
        NEXT_PC;
    });
    /* t = CSRs[csr]; CSRs[csr] = t | x[rs1]; x[rd] = t */
    DEF_I_INST (CSRRS, 0x2, 0x73, {
        ILLGAL_INST_TESTER;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, t0, &t0, RW_R32);
        t1 = t0 | X[RS1];
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, t1, &t1, RW_W32);
        if (stat) FORCE_RET;
        X[RD] = t0;
        NEXT_PC;
    });
    /* t = CSRs[csr]; CSRs[csr] = t &∼x[rs1]; x[rd] = t */
    DEF_I_INST (CSRRC, 0x3, 0x73, {
        ILLGAL_INST_TESTER;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, t0, &t0, RW_R32);
        t1 = t0 &~ X[RS1];
        stat |= csrrw_sync (req.env, IMM_I_ZEXT,t1, &t1, RW_W32);
        if (stat) FORCE_RET;
        X[RD] = t0;
        NEXT_PC;
    });

    DEF_I_INST (CSRRWI, 0x5, 0x73, {
        ILLGAL_INST_TESTER;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, t0, &t0, RW_R32);
        t1 = UIMM;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT,t1, &t1, RW_W32);
        if (stat) FORCE_RET;
        X[RD] = t0;
        NEXT_PC;
    });
    DEF_I_INST (CSRRSI, 0x6, 0x73, {
        ILLGAL_INST_TESTER;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, t0, &t0, RW_R32);
        t1 = t0 | UIMM;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT,t1, &t1, RW_W32);
        if (stat) FORCE_RET;
        X[RD] = t0;
        NEXT_PC;
    });
    DEF_I_INST (CSRRCI, 0x7, 0x73, {
        ILLGAL_INST_TESTER;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT, t0, &t0, RW_R32);
        t1 = t0 &~ UIMM;
        stat |= csrrw_sync (req.env, IMM_I_ZEXT,t1, &t1, RW_W32);
        if (stat) FORCE_RET;
        X[RD] = t0;
        NEXT_PC;
    });
    DEF_UNIQUE_INST (MRET, 0x30200073, {
        if (mstatus_mpp (this->csr.mstatus) != CPU_MODE_U &&
            mstatus_mpp (this->csr.mstatus) != CPU_MODE_M) {
            raise_excep_sync (req.env, EXCP_VMNIMPL);
            stat |= ERR_NOIMPL;
            FORCE_RET;
        }
        // 切换处理器特权级
        req.env->cpu.mode = mstatus_mpp (this->csr.mstatus);
        // mpie->mie
        set_mstatus_mie(this->csr.mstatus, mstatus_mpie (this->csr.mstatus));
        // PC指向中断发生位置
        PC = this->csr.mepc;
        csr_apply (&req.env->cpu, this);
        //printf ("MRET, to mode:%x", req.env->cpu.mode);
        //getchar ();
        // printf ("MRET.\n");
    });
    /* NOIMPL */
    DEF_UNIQUE_INST (WFI, 0x10500073, {});
    DEF_R_INST (SFENCE_VMA, 0x9, 0x0, 0x73, {});
    DEF_UNIQUE_INST (FENCE_I, 0x100F, {});
    DEF_UNIQUE_INST (EBREAK, 0x100073, {});
    DEF_UNIQUE_INST (URET, 0x200073, {});
    DEF_UNIQUE_INST (SRET, 0x10200073, {});
    usermode:
    DEF_UNIQUE_INST (ECALL, 0x73, {
        //printf ("ECALL, mie:%d arg:%x, %x\n", mstatus_mie(this->csr.mstatus) ,req.env->cpu.regs[x10], req.env->cpu.regs[x11]);
        //fflush (stdout);
        stat |= raise_excep_sync (req.env, EXCP_SYSCALL_U);
        //getchar ();
    });
    // stat |= ERR_DECODER_UNDEF;
    goto error;
    executed:
    req.rstat->req_ack = true;
    return stat;
    error:
    return stat;
    #undef t0
    #undef t1
}

static void
csr_apply (struct rv32cpu* cpu, struct rv32g_ctx * this) {
    (void) cpu;
    (void) this;
    cpu->intrp.enable = mstatus_mie(this->csr.mstatus);
    cpu->mtimcmp = this->csr.mtimecmp;
}

static void
csr_refresh (struct rv32cpu* cpu, struct rv32g_ctx * this) {
    if (cpu->intrp.enable)
        set_mstatus_mie(this->csr.mstatus, 1);
    else
        clr_mstatus_mie(this->csr.mstatus);
}

status
rv32g_exhdlr (struct cpu_exhdlr_req req) {
    struct rv32g_ctx * this = (struct rv32g_ctx *) req.ctx;
    status stat = STAT_SUCCESS;
    // MPIE域的值更新为MIE的值
    set_mstatus_mpie (this->csr.mstatus,mstatus_mie(this->csr.mstatus));
    // MIE的值更新为0
    set_mstatus_mie (this->csr.mie, 0);
    // MPP的值被更新为异常发生前的模式
    set_mstatus_mpp (this->csr.mstatus,req.env->cpu.mode);
    // NOTE: 在core中已实现中断后关中断
    this->csr.mcause = req.cause;
    if (req.source != 0)
        this->csr.mcause |= EXCP_ASYNC_SIG;
    DBG_LOG (rv32g_exhdlr,"Caught Exception: src:%d cause:%d, mcause:%d\n", req.source, req.cause, this->csr.mcause);
    // mepc设置为当前PC
    this->csr.mepc = req.env->cpu.pc;
    // PC设置为mtvec
    req.env->cpu.pc = this->csr.mtvec;
    // 切换为机器模式
    req.env->cpu.mode = CPU_MODE_M;
    csr_apply (&req.env->cpu, this);
    req.rstat->req_ack = true;
    return stat;
}

status
rv32zicsr_csrrw (struct cpu_csrrw_req req) {
    struct rv32g_ctx * this = (struct rv32g_ctx *) req.ctx;
    status stat = STAT_SUCCESS;
    if (req.env->cpu.mode != CPU_MODE_M) goto nop;
    csr_refresh (&req.env->cpu, this);
    switch (req.addr) {
        case CSR_MSTATUS: {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mstatus;
            } 
            if (IS_WMODE(req.rw)) {
                this->csr.mstatus = req.send;
                csr_apply (&req.env->cpu, this);
            }
            break;
        }
        case CSR_MIE: {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mie;
            } 
            if (IS_WMODE(req.rw)) {
                this->csr.mie = req.send;
                csr_apply (&req.env->cpu, this);
            }
            break;
        }
        case CSR_MTVEC: {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mtvec;
            } 
            if (IS_WMODE(req.rw)) {
                this->csr.mtvec = req.send;
                csr_apply (&req.env->cpu, this);
            }
            break;
        }
        case CSR_MCAUSE: {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mcause;
            } 
            if (IS_WMODE(req.rw)) {
                this->csr.mcause = req.send;
            }
            break;
        }
        case CSR_MSCRATCH: {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mscratch;
            }
            if (IS_WMODE(req.rw)){
                this->csr.mscratch = req.send;
            }
            break;
        }
        case CSR_MEPC: {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mepc;
            } 
            if (IS_WMODE(req.rw)) {
                this->csr.mepc = req.send;
            }
            break;
        }
        case CSR_MTVAL: {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mtval;
            } 
            if (IS_WMODE(req.rw)) {
                this->csr.mtval = req.send;
            }
            break;
        }
        case CSR_MIP:{
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.mip;
            } 
            if (IS_WMODE(req.rw)) {
                this->csr.mip = req.send;
                csr_apply (&req.env->cpu, this);
            }
            break;
        }
        case CSR_MTIME:{
            if (IS_RMODE(req.rw)) {
                this->csr.mtime = req.env->cpu.mtime;
                *req.recv = this->csr.mtime;
            }
            break;
        }
        case CSR_MTIMECMP:{
            if (IS_RMODE(req.rw)) {
                this->csr.mtimecmp = req.env->cpu.mtimcmp;
                *req.recv = this->csr.mtimecmp;
            }
            if (IS_WMODE(req.rw)) {
                // 偷懒了.写入MTIMECMP时自动开时钟中断
                this->csr.mtimecmp = req.send;
                req.env->cpu.mtimcmp = this->csr.mtimecmp;
                req.env->cpu.mtie = true; // 开启时钟中断使能
            }
            break;
        }
        case CSR_TESTNUM : {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.test;
            }
            if (IS_WMODE(req.rw)) {
                this->csr.test = req.send;
                printf ("TESTING NUM :%d 0x%X\n", req.send, req.send);
            }
            break;
        }
        case CSR_TESTSTR : {
            if (IS_RMODE(req.rw)) {
                *req.recv = this->csr.test;
            }
            if (IS_WMODE(req.rw)) {
                this->csr.test = req.send;
                printf ("TESTING STR :%s @ 0x%X\n", req.env->mem.data + req.send, req.send);
            }
            break;
        }
        case CSR_HALT:{
            if (IS_WMODE(req.rw) && req.send != 0) {
                printf ("\n...HALT...\n");
                env_ctrl (req.env, ENV_CTL_STOP);
                /* 暂时先这样吧 >_< */
                // exit (EXIT_SUCCESS);
            }
            break;
        }
        case CSR_MATP:{
            if (IS_WMODE(req.rw)) {
                this->csr.matp = req.send;
                if (this->csr.matp) {
                    this->vm_enable = true;
                    // printf ("vm logger: pagetable 0x%X setted, refresh virtual memory.\n", this->csr.matp);
                } else {
                    this->vm_enable = false;
                }
            }
            /* 在此模块中仅观察MATP寄存器,不作修改 */
            goto nop;
            break;
        }
        default:
            goto nop;
            break;
    }
    req.rstat->req_ack = true;
    return stat;
    nop:
    return stat;
}